#include "BodyScaler.h"

NS_B2D_BEGIN

typedef void (*ScaleFunction)(b2Shape&, const vec2&, bool);

void ScaleCircle(b2Shape& shape, const vec2& scale, bool reorder)
{
	// Circles can only be scaled uniformly because otherwise they would
	// become an ellipse which isnt supported by Box2D. Scaling circles
	// should be avoided but this will use the x scale for scaling the
	// radius.
	b2CircleShape& circle = static_cast<b2CircleShape&>(shape);
	circle.m_p.x *= scale.x;
	circle.m_p.y *= scale.y;
	circle.m_radius *= fabs(scale.x);
}

void ScaleEdge(b2Shape& shape, const vec2& scale, bool reorder)
{
	b2EdgeShape& edge = static_cast<b2EdgeShape&>(shape);
	// Mandatory points
	edge.m_vertex1.x *= scale.x;
	edge.m_vertex1.y *= scale.y;
	edge.m_vertex2.x *= scale.x;
	edge.m_vertex2.y *= scale.y;
	// Optional points
	if(edge.m_hasVertex0)
	{
		edge.m_vertex0.x *= scale.x;
		edge.m_vertex0.y *= scale.y;
	}
	if(edge.m_hasVertex3)
	{
		edge.m_vertex3.x *= scale.x;
		edge.m_vertex3.y *= scale.y;
	}
}


void ScalePolygon(b2Shape& shape, const vec2& scale, bool reorder)
{
	b2PolygonShape& poly = static_cast<b2PolygonShape&>(shape);
	size_t vcount = (size_t)max(0, poly.m_vertexCount);
	auto verts = poly.m_vertices;

	// Scale
	for(size_t i = 0; i < vcount; ++i)
	{
		verts[i].x *= scale.x;
		verts[i].y *= scale.y;
	}
	// Reorder
	if(reorder)
	{
		size_t hvcount = (size_t)((float)vcount * 0.5f);
		for(size_t i = 0; i < hvcount; ++i)
		{
			size_t j = vcount-i-1;
			auto tmp = verts[i];
			verts[i] = verts[j];
			verts[j] = std::move(tmp);
		}
	}
	// Set the new vertices
	poly.Set(verts, (int32)vcount);
}


void ScaleChain(b2Shape& shape, const vec2& scale, bool reorder)
{
	b2ChainShape& chain = static_cast<b2ChainShape&>(shape);
	
	// Scale vertices
	size_t count = (size_t)max(0, chain.m_count);
	for(size_t i = 0; i < count; ++i)
	{
		chain.m_vertices[i].x *= scale.x;
		chain.m_vertices[i].y *= scale.y;
	}

	// Scale ends if necessary
	if(chain.m_hasNextVertex)
	{
		chain.m_nextVertex.x *= scale.x;
		chain.m_nextVertex.y *= scale.y;
	}
	if(chain.m_hasPrevVertex)
	{
		chain.m_prevVertex.x *= scale.x;
		chain.m_prevVertex.y *= scale.y;
	}
}

void BodyScaler::Scale(b2Body& body, const vec2& scale)
{
	static const ScaleFunction functions[b2Shape::e_typeCount] =
	{
		ScaleCircle,
		ScaleEdge,
		ScalePolygon,
		ScaleChain
	};

	// Polygons require coordinates in CCW order so if we scale one (and only one) coordinate
	// negatively, we must reorder the vertices.
	bool reorder = (scale.x < 0 || scale.y < 0) && !(scale.x < 0 && scale.y < 0); 

	// Loop through fixtures
	auto fixture = body.GetFixtureList();
	while(fixture != nullptr)
	{
		// Get type of shape and scale it
		auto shape = fixture->GetShape();
		size_t type = static_cast<size_t>(shape->GetType());
		functions[type](*shape, scale, reorder);

		// Next fixture
		fixture = fixture->GetNext();
	}

	// The scaling invalidated the mass data
	// so let Box2D recompute it.
	body.ResetMassData();
}

NS_B2D_END